/***************************************************************************************************************************
* command_to_mavros.h
*
* Author: Wr
*
* Update Time: 2021.3.26
*
* Fuction :  无人机控制指令处理  通过mavros包
*         1. 接收控制指令发送至飞控 (setpoint_raw, loca_position, imu and etc..)
*         2.发布控制节点生成的控制量至mavros功能包，可发送期望位置、速度、角度、角速度、底层控制等。
*         3.订阅mavros功能包发布的飞控期望状态量（包括PX4中的期望位置、速度、角度、角速度、底层控制），用于检查飞控是否正确接收机载电脑的指令
*         4.解锁上锁、修改模式两个服务。
***************************************************************************************************************************/
#ifndef COMMAND_TO_MAVROS_H
#define COMMAND_TO_MAVROS_H

#include <ros/ros.h>

#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/PositionTarget.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/ActuatorControl.h>
#include <sensor_msgs/Imu.h>

#include <math_utils.h>

#include <px4_msgs/Px4State.h>
#include <px4_msgs/Px4AttitudeReference.h>

using namespace std;

class command_to_mavros
{
    public:
    command_to_mavros(void):command_nh("~")   //构造函数
    {
        //初始化期望
        pos_px4_target = Eigen::Vector3d(0.0,0.0,0.0);
        vel_px4_target = Eigen::Vector3d(0.0,0.0,0.0);
        accel_px4_target = Eigen::Vector3d(0.0,0.0,0.0);
        q_px4_target = Eigen::Quaterniond(0.0,0.0,0.0,0.0);
        euler_px4_target = Eigen::Vector3d(0.0,0.0,0.0);
        rates_px4_target = Eigen::Vector3d(0.0,0.0,0.0);
        Thrust_target = 0.0;
        
        // 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系
        position_target_sub = command_nh.subscribe<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/target_local", 10, &command_to_mavros::pos_target_cb,this);

        // 【订阅】无人机期望角度/角速度 坐标系:ENU系
        attitude_target_sub = command_nh.subscribe<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/target_attitude", 10, &command_to_mavros::att_target_cb,this);

        // 【订阅】无人机底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力
        actuator_target_sub = command_nh.subscribe<mavros_msgs::ActuatorControl>("/mavros/target_actuator_control", 10, &command_to_mavros::actuator_target_cb,this);

        // 【发布】位置/速度/加速度期望值 坐标系 ENU系
        setpoint_raw_local_pub = command_nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);

        // 【发布】速度期望值 
        setpoint_velocity_pub = command_nh.advertise<geometry_msgs::Twist>("/mavros/setpoint_velocity/cmd_vel_unstamped", 10);

        // 【发布】角度/角速度期望值 坐标系 ENU系
        setpoint_raw_attitude_pub = command_nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);

        // 【发布】底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力 
        //   注意： 这里是NED系的！！
        actuator_setpoint_pub = command_nh.advertise<mavros_msgs::ActuatorControl>("/mavros/actuator_control", 10);

        // 【服务】解锁/上锁
        arming_client = command_nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");

        // 【服务】修改系统模式
        set_mode_client = command_nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");

    }

    //订阅px4期望的状态量话题，赋值到下列各变量   表示被设置的期望值
    //Target pos of the px4 [from px4]
    Eigen::Vector3d pos_px4_target;
    //Target vel of the px4 [from px4]
    Eigen::Vector3d vel_px4_target;
    //Target accel of the px4 [from px4]
    Eigen::Vector3d accel_px4_target;
    //Target att of the px4 [from px4]
    Eigen::Quaterniond q_px4_target;
    Eigen::Vector3d euler_px4_target;
    Eigen::Vector3d rates_px4_target;
    //Target thrust of the px4 [from px4]
    float Thrust_target;
    mavros_msgs::ActuatorControl actuator_target;

     //模式修改与加解锁服务所用变量声明
    mavros_msgs::SetMode mode_cmd;
    mavros_msgs::CommandBool arm_cmd;
    //ros服务声明
    ros::ServiceClient arming_client;
    ros::ServiceClient set_mode_client;

    //Idle. Do nothing.
    void idle();

    //发送位置期望值至飞控（输入：期望xyz,期望yaw）
    void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp);

    //发送速度期望值至飞控（输入：期望vxvyvz）
    void send_vel_unstamped_setpoint(const Eigen::Vector3d& vel_sp);

    //发送速度期望值至飞控（输入：期望vxvyvz,期望yaw）
    void send_vel_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp);

    //发送速度期望值至飞控（输入：期望vxvy z,期望yaw）
    void send_vel_xy_pos_z_setpoint(const Eigen::Vector3d& state_sp, float yaw_sp);

    //发送速度期望值至飞控（机体系）（输入：期望vxvyvz,期望yaw）
    void send_vel_setpoint_body(const Eigen::Vector3d& vel_sp, float yaw_sp);

    //发送加速度期望值至飞控（输入：期望axayaz,期望yaw）
    void send_accel_setpoint(const Eigen::Vector3d& accel_sp, float yaw_sp);

    //发送角度期望值至飞控（输入：期望角度-四元数,期望推力）
     //这是px4_pose_controller.cpp中目前使用的控制方式？？
    void send_attitude_setpoint(const px4_msgs::Px4AttitudeReference& _AttitudeReference);

    //发送角度期望值至飞控（输入：期望角速度,期望推力）
    void send_attitude_rate_setpoint(const Eigen::Vector3d& attitude_rate_sp, float thrust_sp);

    //发送底层至飞控（输入：MxMyMz,期望推力）[Not recommanded. Because the high delay between the onboard computer and Pixhawk]
    void send_actuator_setpoint(const Eigen::Vector4d& actuator_sp);

    private:
        ros::NodeHandle command_nh;

        //创建订阅者  订阅mavros话题
        ros::Subscriber position_target_sub;
        ros::Subscriber attitude_target_sub;
        ros::Subscriber actuator_target_sub;
        //创建发布者  发布期望控制量至mavros
        ros::Publisher setpoint_raw_local_pub;
        ros::Publisher setpoint_raw_attitude_pub;
        ros::Publisher actuator_setpoint_pub;
        ros::Publisher setpoint_velocity_pub;

        //   回调函数
        //  主要检查飞控是否接受到控制指令
        void pos_target_cb(const mavros_msgs::PositionTarget::ConstPtr& msg)  //消息来自飞控，表示被设置的位置速度加速度目标值
        {
            pos_px4_target = Eigen::Vector3d(msg->position.x, msg->position.y, msg->position.z);

            vel_px4_target = Eigen::Vector3d(msg->velocity.x, msg->velocity.y, msg->velocity.z);

            accel_px4_target = Eigen::Vector3d(msg->acceleration_or_force.x, msg->acceleration_or_force.y, msg->acceleration_or_force.z);
        }

        void att_target_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg)//消息来自飞控，表示被设置的角度和角速度目标值
        {
            q_px4_target = Eigen::Quaterniond(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
            //Transform the Quaternion to euler Angles
            //通过math_utils工具实现
            euler_px4_target = quaternion_to_euler(q_px4_target);  
            rates_px4_target = Eigen::Vector3d(msg->body_rate.x, msg->body_rate.y, msg->body_rate.z);
            Thrust_target = msg->thrust;
        }

        void actuator_target_cb(const mavros_msgs::ActuatorControl::ConstPtr& msg)//消息来自飞控，表示被设置的底层控制量目标值
        {
            actuator_target = *msg;
        }
};


//***************************************************************************************************************************

void command_to_mavros::idle()
{
    mavros_msgs::PositionTarget pos_setpoint;

    //Here pls ref to mavlink_receiver.cpp
    //这里type_mask设置为0x4000是什么模式？ 按照预定这里应该是怠速状态，可以切换Offboard模式和解锁
    //这时发送的期望位姿是什么？
    pos_setpoint.type_mask = 0x4000;  //0x4000查看PositionTarget.msg 消息

    setpoint_raw_local_pub.publish(pos_setpoint);
}

//发送位置期望值至飞控（输入：期望xyz,期望yaw）
void command_to_mavros::send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)
{
    mavros_msgs::PositionTarget pos_setpoint;
    //Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)
    //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
    //Bit 10 should set to 0, means is not force sp
    pos_setpoint.type_mask = 0b100111111000;  // 100 111 111 000  xyz + yaw

    pos_setpoint.coordinate_frame = 1;  //FRAME_LOCAL_NED  ？

    pos_setpoint.position.x = pos_sp[0];
    pos_setpoint.position.y = pos_sp[1];
    pos_setpoint.position.z = pos_sp[2];

    pos_setpoint.yaw = yaw_sp;

    setpoint_raw_local_pub.publish(pos_setpoint);

}

 //发送速度期望值至飞控（输入：期望vxvyvz）
void  command_to_mavros::send_vel_unstamped_setpoint(const Eigen::Vector3d& vel_sp)
{
    geometry_msgs::Twist vel_setpoint;
    vel_setpoint.linear.x = vel_sp[0];
    vel_setpoint.linear.y = vel_sp[1];
    vel_setpoint.linear.z = vel_sp[2];
    setpoint_velocity_pub.publish(vel_setpoint);
}


//发送期望vx，vy，z，和yaw至飞控
void command_to_mavros::send_vel_xy_pos_z_setpoint(const Eigen::Vector3d& state_sp, float yaw_sp)
{
    mavros_msgs::PositionTarget pos_setpoint;
    //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
    pos_setpoint.type_mask = 0b100111000011;   // z + vx vy vz 下面vz设置成了0  所以是vx vy + z 模式

    pos_setpoint.coordinate_frame = 1;

    pos_setpoint.velocity.x = state_sp[0];
    pos_setpoint.velocity.y = state_sp[1];
    pos_setpoint.velocity.z = 0.0;
    pos_setpoint.position.z = state_sp[2];

    pos_setpoint.yaw = yaw_sp;

    setpoint_raw_local_pub.publish(pos_setpoint);
}

//发送期望vx   vy   vz,期望yaw至飞控
void command_to_mavros::send_vel_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp)
{
    mavros_msgs::PositionTarget pos_setpoint;
    //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
    pos_setpoint.type_mask = 0b100111000111;  // vx vy vz  + yaw模式

    pos_setpoint.coordinate_frame = 1;

    pos_setpoint.velocity.x = vel_sp[0];
    pos_setpoint.velocity.y = vel_sp[1];
    pos_setpoint.velocity.z = vel_sp[2];

    pos_setpoint.yaw = yaw_sp;

    setpoint_raw_local_pub.publish(pos_setpoint);
    
}

//发送速度期望vx  vy   vz,  期望yaw至飞控
// 机体系
void command_to_mavros::send_vel_setpoint_body(const Eigen::Vector3d& vel_sp, float yaw_sp)
{
    mavros_msgs::PositionTarget pos_setpoint;
    //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
    pos_setpoint.type_mask = 0b100111000111;   // vx vy vz  + yaw模式

    //uint8 FRAME_LOCAL_NED = 1
    //uint8 FRAME_BODY_NED = 8
    pos_setpoint.coordinate_frame = 8;   //这里坐标系使用FRAME_BODY_NED

    pos_setpoint.position.x = vel_sp[0];
    pos_setpoint.position.y = vel_sp[1];
    pos_setpoint.position.z = vel_sp[2];

    pos_setpoint.yaw = yaw_sp;

    setpoint_raw_local_pub.publish(pos_setpoint);

}

//发送期望加速度ax  ay  az,  期望yaw至飞控
void command_to_mavros::send_accel_setpoint(const Eigen::Vector3d& accel_sp, float yaw_sp)
{
    mavros_msgs::PositionTarget pos_setpoint;

    pos_setpoint.type_mask = 0b100000111111;  // ax  ay  az + yaw 模式

    pos_setpoint.coordinate_frame = 1;

    pos_setpoint.acceleration_or_force.x = accel_sp[0];
    pos_setpoint.acceleration_or_force.y = accel_sp[1];
    pos_setpoint.acceleration_or_force.z = accel_sp[2];

    pos_setpoint.yaw = yaw_sp;

    setpoint_raw_local_pub.publish(pos_setpoint);

}

//发送角度期望值至飞控
//输入：期望角度-四元数,   期望推力
void command_to_mavros::send_attitude_setpoint(const px4_msgs::Px4AttitudeReference& _AttitudeReference)
{
    mavros_msgs::AttitudeTarget att_setpoint;

    //Mappings: If any of these bits are set, the corresponding input should be ignored:
    //bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude

    att_setpoint.type_mask = 0b00000111;

    att_setpoint.orientation.x = _AttitudeReference.desired_att_q.x;
    att_setpoint.orientation.y = _AttitudeReference.desired_att_q.y;
    att_setpoint.orientation.z = _AttitudeReference.desired_att_q.z;
    att_setpoint.orientation.w = _AttitudeReference.desired_att_q.w;

    att_setpoint.thrust = _AttitudeReference.desired_throttle;

    setpoint_raw_attitude_pub.publish(att_setpoint);

}

// 输入：期望角速度,    期望推力
void command_to_mavros::send_attitude_rate_setpoint(const Eigen::Vector3d& attitude_rate_sp, float thrust_sp)
{
    mavros_msgs::AttitudeTarget att_setpoint;

    //Mappings: If any of these bits are set, the corresponding input should be ignored:
    //bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude

    att_setpoint.type_mask = 0b10000000;  //忽略姿态控制

    att_setpoint.body_rate.x = attitude_rate_sp[0];
    att_setpoint.body_rate.y = attitude_rate_sp[1];
    att_setpoint.body_rate.z = attitude_rate_sp[2];

    att_setpoint.thrust = thrust_sp;

    setpoint_raw_attitude_pub.publish(att_setpoint);

}


//发送底层至飞控（输入：MxMyMz,期望推力）
void command_to_mavros::send_actuator_setpoint(const Eigen::Vector4d& actuator_sp)
{
    mavros_msgs::ActuatorControl actuator_setpoint;

    actuator_setpoint.group_mix = 0;
    actuator_setpoint.controls[0] = actuator_sp(0);
    actuator_setpoint.controls[1] = actuator_sp(1);
    actuator_setpoint.controls[2] = actuator_sp(2);
    actuator_setpoint.controls[3] = actuator_sp(3);
    actuator_setpoint.controls[4] = 0.0;
    actuator_setpoint.controls[5] = 0.0;
    actuator_setpoint.controls[6] = 0.0;
    actuator_setpoint.controls[7] = 0.0;

    actuator_setpoint_pub.publish(actuator_setpoint);

    // // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_mavros<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // //ned to enu
    // cout << "actuator_target [0 1 2 3] : " << actuator_target.controls[0] << " [ ] "<< -actuator_target.controls[1] <<" [ ] "<<-actuator_target.controls[2]<<" [ ] "<<actuator_target.controls[3] <<" [ ] "<<endl;

    // cout << "actuator_target [4 5 6 7] : " << actuator_target.controls[4] << " [ ] "<< actuator_target.controls[5] <<" [ ] "<<actuator_target.controls[6]<<" [ ] "<<actuator_target.controls[7] <<" [ ] "<<endl;

}

#endif // !COMMAND_TO_MAVROS_H

